/**
 * @file           : DjiSnail.h
 * @author         : Star_Plucking
 * @brief          : None
 * @date           : 25-1-11 下午4:35
 * @lastEditor     :
 * @lastEditTime   : 25-1-11 下午4:35
 */

#pragma once
#include <sheriffos.h>

namespace device::dji_snail {
class Encoder {
    float m_RawAngle = 0;           ///< (单位：角度)电机编码器返回角0-360（不含角度偏移）
    float m_Speed = 0;              ///< (单位：rpm)电机编码器返回速度
    float m_LinearVelocity = 0;     ///< (单位：m/s)电机计算得到线速度
    int16_t m_Turns = 0;            ///< 电机编码器返回圈数
    int16_t m_LastTurns = 0;        ///< 上次圈数
    float m_LastAngel = 0;          ///< 上次角度
    uint64_t m_LastUpdateTime = 0;  ///< (单位：毫秒) 上次更新时间
    SPI_HandleTypeDef* m_hspi{};    ///< 编码器spi句柄
    GPIO_TypeDef* m_cs_port{};      ///< 编码器cs引脚端口
    uint16_t m_cs_pin{};            ///< 编码器cs引脚编号
    uint8_t m_data[4]{};            ///< 编码器接收数据

    float m_diameter = 0.0F;  ///< (单位：m)电机轮直径

public:
    Encoder() = default;

    Encoder(SPI_HandleTypeDef* spi, GPIO_TypeDef* cs_port, uint16_t cs_pin, float diameter)
        : m_hspi(spi), m_cs_port(cs_port), m_cs_pin(cs_pin), m_diameter(diameter) {}

    ~Encoder() = default;

    /**
     * @brief 编码器初始化
     * @note 初始化大约需要0.2s左右
     */
    void Init() {
        uint8_t tx_buffer[4] = {};
        // HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        // osDelay(1);
        // HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);

        tx_buffer[0] = 0x00;
        tx_buffer[1] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        osDelay(1);

        tx_buffer[0] = 0x00;
        tx_buffer[1] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        osDelay(1);

        tx_buffer[0] = 0x00;
        tx_buffer[1] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        osDelay(1);

        tx_buffer[0] = 0xea;
        tx_buffer[1] = 0x54;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        osDelay(1);

        tx_buffer[0] = 0x1c;
        tx_buffer[1] = 0b00000000;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        HAL_Delay(1);

        tx_buffer[0] = 0x00;
        tx_buffer[1] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        HAL_Delay(1);

        tx_buffer[0] = 0x00;
        tx_buffer[1] = 0x00;
        tx_buffer[2] = 0x00;
        tx_buffer[3] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 4, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        HAL_Delay(1);

        tx_buffer[0] = 0xea;
        tx_buffer[1] = 0x54;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        HAL_Delay(1);

        tx_buffer[0] = 0x0D;
        tx_buffer[1] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        HAL_Delay(1);

        tx_buffer[0] = 0x00;
        tx_buffer[1] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 2, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        HAL_Delay(1);

        tx_buffer[0] = 0x00;
        tx_buffer[1] = 0x00;
        tx_buffer[2] = 0x00;
        tx_buffer[3] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 4, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);
        HAL_Delay(1);
    }

    void EncoderDataProcess(uint8_t const* buffer) {
        uint64_t now_time = bsp::getGlobalTime_us_i64();
        // 读取编码器角度值（0-65535范围）
        auto encoderRawData = static_cast<int16_t>(buffer[0] << 8 | buffer[1]);
        // 将编码器值转换为-180°~180°的角度
        m_RawAngle = static_cast<float>(encoderRawData) * 360.0f / 65536.0f - 180.0f;
        // 读取多圈信息
        m_Turns = static_cast<int16_t>((buffer[2] << 8) | buffer[3]);
        // 计算角度的差值，考虑过零点情况
        float angle_diff = m_RawAngle - m_LastAngel;
        if (angle_diff > 180.0f) {
            // 说明角度从负值跳到正值（即过零点）
            angle_diff -= 360.0f;
        } else if (angle_diff < -180.0f) {
            // 说明角度从正值跳到负值（即过零点）
            angle_diff += 360.0f;
        }
        if (m_Speed > 400 && angle_diff < 0) {
            angle_diff += 360.0f;
        } else if (m_Speed < -400 && angle_diff > 0) {
            angle_diff -= 360.0f;
        }
        // 根据角度差值判断圈数变化
        if (angle_diff > 0 && m_LastAngel < -90 && m_RawAngle > 90) {
            // 过零点（从负数跨到正数），圈数加一
            m_Turns++;
        } else if (angle_diff < 0 && m_LastAngel > 90 && m_RawAngle < -90) {
            // 过零点（从正数跨到负数），圈数减一
            m_Turns--;
        }
        // 将角度差转换为角速度 (rad/s)
        auto sampling_period = static_cast<float>(now_time - m_LastUpdateTime) / 1000000.0f;  // s
        m_LastUpdateTime = now_time;
        m_Speed = (angle_diff / 180.0f * 3.1415926f) / sampling_period;  // rad/s
        // 根据角速度计算线速度 (m/s)
        m_LinearVelocity = m_Speed * m_diameter / 2.0f;
        // 保存上次的角度和多圈数
        m_LastAngel = m_RawAngle;
        m_LastTurns = m_Turns;
    }

    void ReadEncoder() {
        uint8_t tx_buffer[4] = {};
        tx_buffer[0] = 0x00;
        tx_buffer[1] = 0x00;
        tx_buffer[2] = 0x00;
        tx_buffer[3] = 0x00;
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(m_hspi, tx_buffer, m_data, 4, 100);
        HAL_GPIO_WritePin(m_cs_port, m_cs_pin, GPIO_PIN_SET);

        EncoderDataProcess(m_data);
    }

    float GetSpeed() const { return m_Speed; }

    float GetLinearVelocity() const { return m_LinearVelocity; }

    float GetAngle() const { return m_RawAngle; }

    int16_t GetTurns() const { return m_Turns; }

    int16_t GetLastTurns() const { return m_LastTurns; }

    uint64_t GetLastUpdateTime() const { return m_LastUpdateTime; }

    void SetDiameter(float diameter) { m_diameter = diameter; }

    float GetDiameter() const { return m_diameter; }
};
}  // namespace device::dji_snail
namespace device::dji_snail {
class Motor {
private:
    TIM_HandleTypeDef* m_htim{};  ///< 电机控制定时器句柄
    uint32_t m_channel{};         ///< 电机控制通道
    float m_output = 0;           ///< 电机输出

public:
    Encoder encoder;
    Motor() = default;

    Motor(TIM_HandleTypeDef* htim, uint32_t channel, SPI_HandleTypeDef* spi, GPIO_TypeDef* cs_port, uint16_t cs_pin,
          float diameter)
        : m_htim(htim), m_channel(channel), encoder(spi, cs_port, cs_pin, diameter) {}

    ~Motor() = default;

    void Init() {
        encoder.Init();
        HAL_TIM_PWM_Start(m_htim, m_channel);
    }

    void InitMotorOnly() { HAL_TIM_PWM_Start(m_htim, m_channel); }

    /**
     * @brief 发送电机控制信号
     */

    void SendOutput() const {
        float duty = m_output * 0.4F / 50 + 0.5F;
        if (duty < 0.5f) {
            duty = 0.5f;
        }
        if (duty > 0.9f) {
            duty = 0.9f;
        }
        auto const Pulse = duty * static_cast<float>(m_htim->Init.Period + 1);
        __HAL_TIM_SetCompare(m_htim, m_channel, Pulse);
    }

    /**
     * @brief Set the Output object
     * @param output
     * @note 电机输出范围为0~20.0f
     */
    void SetOutput(float const output) { m_output = output; }
};
}  // namespace device::dji_snail